#include <string>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/Range.h>

#include "Driver.h"
#include "./obstacles/RandomWalk.h"
#include "WallFollower.h"
#include "./objectTracker/ColorTracker.h"

#include "../utilities/MsgParser.h"
#include "../utilities/Constants.h"
#include  "../utilities/ObjPosition.h"

#include "../ArmMngrNode/ArmMover.h"

#include <map>
#include "../utilities/XmlParser.h"

// libraries for kill signal handling
#include <signal.h>
#include <unistd.h>

#define CM_TO_M 100 // translate centimeters to meters
#define ARM_DIST_SCALE 30 // arm catching constant scale in centimeters from front side of the robot
#define COMMANDS_QUEUE_SIZE 1000
#define CAMERA_QUEUE_SIZE 5

using namespace std;

// object in charge of publishing commands to wheels- will be allocated in main
Driver* driver;
ArmMover* arm;
WallFollower* follower;
ColorTracker* color_tracker;
RandomWalk* rand_walk;

string task_name; // name of task to perform (set by DefaultTask in params.xml)

// ROS management objects
ros::Rate* loop_rate;
ros::Publisher sensors_publisher;
ros::Publisher color_pub;
std_msgs::String message_to_publish;

double speed; // linear speed (meter/second) for running the task
double side_distance; // distance (meters) to keep from wall while running wall following

// lower and upper bounds of color to detect
int lower_h;
int lower_s;
int lower_v;
int upper_h;
int upper_s;
int upper_v;

// parameters parsed from params.xml
map<string, string> params;

void trackObject();
void followWall();

void parseParams(float& speed, int& h1, int& s1, int& v1, int& h2, int& s2, int& v2);
void setTrackingObjColor(int h1, int s1, int v1, int h2, int s2, int v2);


/**
 * Runs the default task (which is set by params.xml)
 * @param msg ROS topic message
 */
void commandsHandler(const std_msgs::String::ConstPtr& msg) {

	// notify
  	ROS_INFO("/Rover_Module/Driver received message.");

  	// initialize arm position
	arm->setStartPosition();
	sleep(2);

  	// run the specified task
  	if (strcmp("ColorTracker",task_name.c_str()) == 0) {

  		trackObject();
  		return;
	}

  	if (strcmp("WallFollower",task_name.c_str()) == 0) {

		followWall();
		return;
	}

}

/**
 * Follows the wall
 */
void followWall() {

	// allocate a wall-following manager object
	follower = new WallFollower(driver, speed, side_distance);

	// start wall following
	follower->follow();

	// done task
	delete follower;
}

/**
 * Tracks the object and stops when reached it.
 * If the arm cannot grab it, then robot backs off and tries again.
 * Once object is grabbed, finds the stop position and stops
 */
void trackObject() {

	// set the color to track (as specified in params.xml)
	setTrackingObjColor(lower_h,lower_s,lower_v,upper_h,upper_s,upper_v);

  	// initialize object to perform the tracking
  	color_tracker = new ColorTracker(driver,rand_walk,speed);

  	// track object until is grabbed successfully
  	while (true) {

  		// get object's final position
  		ObjPosition obj_pos = color_tracker->track();

  		// try launching the arm towards the object
  		bool check_moving = arm->moveArm(CM_TO_M*obj_pos.m_dist + ARM_DIST_SCALE, -CM_TO_M*obj_pos.m_x, CM_TO_M*obj_pos.m_y+1);

  		// check if arm has reached the object
		if (!check_moving) {
			continue; // try again
		}
  		sleep(2);

  		// try grabbing the object
  		bool check_catching = arm->squeezeObject();

  		// check if could grab the object
		if (check_catching) {
			break; // success
		}

  	}

  	// pick up the object
  	sleep(2);
  	arm->liftObject();

  	// drive backwards
	driver->move(-speed, 0);
	sleep(1);

	// turn 180 degrees
	driver->move(0, 2.5);
	sleep(2);

	// Find stop position
	ObjPosition obj_pos = color_tracker->track(0.9, 1.0);

	// done task
  	delete color_tracker;
}

/**
 * Handler for SIGTERM handler- stops robot when node is killed with ctr+c
 * @param signum signal to react
 * @param info signal's data
 * @param ptr pointer to signal handler
 */
void emergencyStop(int signum, siginfo_t *info, void *ptr) {

	ROS_ERROR("emergency stop");

	if (driver == NULL) {
		ROS_INFO("driver is null");
		return;
	}

	// stop the robot
	for (int i = 0; i < 3; ++i) {

			driver->stop();
			driver->stop();
			driver->stop();
			driver->stop();
			sleep(1);
	}

	delete driver;
	delete arm;
	delete rand_walk;

	ros::shutdown();
}

/**
 * Registers SIGTERM handler
 */
void registerKillSignalCB() {

	// act for each signal
	struct sigaction act_SIGINT;

	// block masks for each signal
	sigset_t block_mask_SIGINT;

	// block other signals while handling SIGUSR2 or SIGALRM
	sigfillset(&block_mask_SIGINT);

	// set signal-action for SIGUSR2 signal
	act_SIGINT.sa_mask = block_mask_SIGINT;
	act_SIGINT.sa_sigaction = emergencyStop;
	act_SIGINT.sa_flags = 0;

	// register handlers
	sigaction(SIGINT, &act_SIGINT, NULL);
}

/**
 * Publishes to vision node the tracked object's color.
 * The color is given as threshold value (lower and upper bounds)
 * in HSV representation
 * @param h1 Hue for lower bound
 * @param s1 Saturation for lower bound
 * @param v1 Value for lower bound
 * @param h2 Hue for upper bound
 * @param s2 Saturation for upper bound
 * @param v2 Value for upper bound
 */
void setTrackingObjColor(int h1, int s1, int v1, int h2, int s2, int v2) {

	// compose message
	char buffer[32];
	snprintf(buffer, sizeof(buffer), "%d%s%d%s%d%s%d%s%d%s%d",
			h1, DELIMITER,
			s1, DELIMITER,
			v1, DELIMITER,
			h2, DELIMITER,
			s2, DELIMITER,
			v2);

	// set topic message
	std_msgs::String message_to_publish;
	message_to_publish.data = buffer;

	ROS_WARN("RoverManagerNode publishes: %s", buffer);

	// publish message with object position
	color_pub.publish(message_to_publish);
}

/**
 * Parses speed and side-distance out of params.xml
 * @param[out] speed value (meter/second) for speed parameter
 * @param[out] side_dist value (meter) for side-distance parameter
 */
void parseParams(double& speed, double& side_distance) {

	speed 	  = atof(params["speed"].c_str());
	side_distance = atof(params["side_distance"].c_str());
}

/**
 * Parses speed and threshold value of color to track out of params.xml
 * @param speed[out] speed value (meter/second) for speed parameter
 * @param h1[out] Hue for lower bound
 * @param s1[out] Saturation for lower bound
 * @param v1[out] Value for lower bound
 * @param h2[out] Hue for upper bound
 * @param s2[out] Saturation for upper bound
 * @param v2[out] Value for upper bound
 */
void parseParams(double& speed, int& h1, int& s1, int& v1, int& h2, int& s2, int& v2) {

	speed = atof(params["speed"].c_str());

	h1 = atoi(params["lower_h"].c_str());
	s1 = atoi(params["lower_s"].c_str());
	v1 = atoi(params["lower_v"].c_str());
	h2 = atoi(params["upper_h"].c_str());
	s2 = atoi(params["upper_s"].c_str());
	v2 = atoi(params["upper_v"].c_str());
}

/**
 * Listens to "driver" topic until shuts down.
 * Parses published messages and uses the call-back function
 * to perform the commands
 */
int main(int argc, char **argv) {

	// initialize system
	ros::init(argc, argv, "RoverMngrNode");
	ros::NodeHandle node_handler;

	loop_rate = new ros::Rate(1);

	// register SIGINT handling
	registerKillSignalCB();

	try {

		// extract parameters from configurations xml file
		XmlParser::extractNodeData("RoverMngr", params);
		task_name = params["DefaultTask"];

		XmlParser::extractNodeData("RoverMngr", task_name, params);

	} catch (const char* e) {

		ROS_ERROR("RoverMngrNode xml parsing error. Exit.");
		return 0;
	}

	// read the parameters from xml file
	if (strcmp("ColorTracker",task_name.c_str()) == 0) {

		parseParams(speed,lower_h,lower_s,lower_v,upper_h,upper_s,upper_v);

	} else if (strcmp("WallFollower",task_name.c_str()) == 0) {

		parseParams(speed, side_distance);
	}

	// allocating the rover controller
	driver = new Driver();
	arm = new ArmMover();
	rand_walk = new RandomWalk(driver, speed);

	// receive commands from somebody (now - terminal)
	ros::Subscriber sub_command_receive = node_handler.subscribe("/Rover_Module/Driver",COMMANDS_QUEUE_SIZE,commandsHandler);

	// publishes color for vision node
	color_pub = node_handler.advertise<std_msgs::String>("/VisionNode/SetColor", CAMERA_QUEUE_SIZE);

	// keep listening to subscribers until ros::shutdown() has been called and is finished
	while (ros::ok()) {
		ros::spinOnce();
		//main_loop_rate.sleep();
	}

	// done listening

	ROS_ERROR("in main, done function");
	driver->stop();
	delete driver;
	delete arm;
	delete rand_walk;
	driver = NULL;
	return 0;
}
